#include "m2dpDescriptor.h"

m2dp_descriptor::m2dp_descriptor(
	int num_T 					= 16,
	int num_R 					= 8,
	int num_P 					= 4,
	int num_Q 					= 16,
	int robot_num				= 1,
	int id						= 0 ) :
    //6个参数
	num_T_(num_T), // number of bins in theta（角度维度）, the 't' in paper
	num_R_(num_R), // number of bins in rho（距离维度）, the 'l' in paper
	num_P_(num_P), // number of azimuth angles（方位角的数量）, the 'p' in paper
	num_Q_(num_Q), // number of elevation angles （仰角的数量）, the 'q' in paper
	robot_num_(robot_num), // number of robot in robotic swarm（机器人数量）
	id_(id) // this robot id（机器人的ID）
{
	// init 用于初始化和分配内存

    // 生成一系列在区间[-π/2, π/2]内均匀分布的角度值，
    // 并赋值给为azimuth_list向量。
	azimuth_list = *new vector<double>(num_P_);
	for(int i = 0; i < num_P_; i++)
	{
		azimuth_list[i] = -M_PI_2 + i * M_PI / (num_P_ - 1);
	}

    // 生成一系列在区间[0, π/2]内均匀分布的角度值
    // 并赋值给为elevation_list向量。
	elevation_list = *new vector<double>(num_Q_);
	for(int i = 0; i < num_Q_; i++)
	{
		elevation_list[i] = i * M_PI_2 / (num_Q_ - 1);
	}

	// allocate memory
    // 为m2dp_dictionaries和indexes_maps分别分配内存
	for(int i = 0; i < robot_num_; i++)
	{
		std::vector<Eigen::VectorXf> base_vector;
		std::vector<int> base_int;
		m2dp_dictionaries.push_back(base_vector);
		indexes_maps.push_back(base_int);
	}
}

m2dp_descriptor::~m2dp_descriptor()
{
	
}

std::vector<float> m2dp_descriptor::makeAndSaveDescriptorAndKey(
	const pcl::PointCloud<pcl::PointXYZI>& scan,
	const int8_t robot,
	const int index)
{
    // 创建一个指向 pcl::PointCloud<pcl::PointXYZ> 对象的智能指针 input_cloud
    // 并将其初始化为空点云。
	pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud;
	input_cloud.reset(new pcl::PointCloud<pcl::PointXYZ>());
    // scan.points 中的每个点，并将其坐标（x、y、z）存储到 input_cloud 中
	for(int i = 0; i < scan.points.size(); i++)
	{
		pcl::PointXYZ p;
		p.x = scan.points[i].x;
		p.y = scan.points[i].y;
		p.z = scan.points[i].z;
		input_cloud->push_back(p);
	}
    // 创建一个指向 pcl::PointCloud<pcl::PointXYZ> 对象的智能指针 cloud_filtered
    // 并将其初始化为空点云
	cloud_filtered.reset(new pcl::PointCloud<pcl::PointXYZ>());
    // 创建一个 pca 对象，
    // 并将 input_cloud 设置为其输入点云
	pcl::PCA<pcl::PointXYZ> pca;
	// 先转换成pca对象
	pca.setInputCloud(input_cloud);
    // 调用 pca.project 方法，将投影后的点云数据存储到 cloud_filtered 中
	/* 
	模板点云点
		template<typename PointT> inline void
	PCA<PointT>::project (const PointT& input, PointT& projection)
	{
	if(!compute_done_)
		initCompute ();
	if (!compute_done_)
		PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::project] PCA initCompute failed");
	// 计算输入点相对于均值点的差值，得到一个 3 维的向量 demean_input。这个向量表示输入点的坐标相对于均值点的偏移。
	Eigen::Vector3f demean_input = input.getVector3fMap () - mean_.head<3> ();
	// 计算投影后的点的坐标，这是通过将均值去掉后的输入点与特征向量的转置相乘得到的。这个计算会将输入点投影到 PCA 的特征向量所构成的子空间上，得到投影点的坐标。
	projection.getVector3fMap () = eigenvectors_.transpose() * demean_input;
	}
	模板类点云
			template<typename PointT> inline void
		PCA<PointT>::project (const PointCloud& input, PointCloud& projection)
		{
		// 初始化
		if(!compute_done_)
			initCompute ();
		// 初始化失败，爆出异常
		if (!compute_done_)
			PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::project] PCA initCompute failed");
		// 判断输入点云是否是稠密的（没有 NaN 或 Inf 值）
		if (input.is_dense)
		{
			projection.resize (input.size ());
			for (std::size_t i = 0; i < input.size (); ++i)
			project (input[i], projection[i]);
		}
		else
		{
			PointT p;
			for (const auto& pt: input)
			{
			// 检查点的坐标是否有无穷大或 NaN 值，如果有，则跳过该点。
			if (!std::isfinite (pt.x) ||
				!std::isfinite (pt.y) ||
				!std::isfinite (pt.z))
				continue;
			project (pt, p);
			projection.push_back (p);
			}
		}
		}
	 */
	pca.project(*input_cloud, *cloud_filtered);

    // 初始化最大距离rho=0
	max_rho = 0;
    // 创建一个大小为 cloud_filtered->points.size() 的矩阵 cloud_pca（n行，三列）
	cloud_pca.resize(cloud_filtered->points.size(), 3);
    // 遍历 cloud_filtered->points 中的每个点，将其坐标（x、y、z）存储到 cloud_pca 中，
    // 并计算最远点的距离（max_rho）
	for(int i = 0; i < cloud_filtered->points.size(); i++)
	{
		cloud_pca(i, 0) = cloud_filtered->points[i].x;
		cloud_pca(i, 1) = cloud_filtered->points[i].y;
		cloud_pca(i, 2) = -cloud_filtered->points[i].z;

		// get the farthest point distance，计算最大距离
		double temp_rho = sqrt(
				cloud_filtered->points[i].x * cloud_filtered->points[i].x
			+ cloud_filtered->points[i].x * cloud_filtered->points[i].x
			+ cloud_filtered->points[i].z * cloud_filtered->points[i].z);

        // 更新最大距离
		if(temp_rho > max_rho)
		{
			max_rho = temp_rho;
		}
	}

	// get the signature matrix A
    // 调用 GetSignatureMatrix 方法，获取特征矩阵 A
	A = GetSignatureMatrix();

    // 创建一个 Eigen::JacobiSVD 对象 svd，并传入 A 和参数 Eigen::ComputeThinU | Eigen::ComputeThinV。
    // 这将计算 svd 对象的矩阵 U 和 V，并将其存储在相应的成员变量 matrixU 和 matrixV 中。
	Eigen::JacobiSVD<Eigen::MatrixXd> svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV);
    // 创建两个 Eigen::MatrixXd 对象 u 和 v，并分别将 svd 对象的 matrixU 和 matrixV 赋值给它们
	Eigen::MatrixXd u = svd.matrixU();
	Eigen::MatrixXd v = svd.matrixV();
    // 创建两个固定大小的 Eigen::Matrix 对象 u_temp 和 v_temp。u_temp 的大小为 1x64，v_temp 的大小为 1x128。
    // 通过 u.col(0) 和 v.col(0)，将 u 和 v 的第一列赋值给 u_temp 和 v_temp。
	Eigen::Matrix<double, 1, 64> u_temp;
	Eigen::Matrix<double, 1, 128> v_temp;
	u_temp = u.col(0);
	v_temp = v.col(0);
    // 将 u_temp 和 v_temp 按列连接成一个大小为 1x192 的 Eigen::Matrix<double, 1, 192> 对象，
    // 并将结果赋值给 m2dp。
	m2dp << u_temp, v_temp;

    // 创建一个空的 std::vector<float> 类型的对象 m2dp_vec，用于存储 m2dp 矩阵的元素
	std::vector<float> m2dp_vec;
    // m2dp 矩阵中的元素 m2dp[i] 添加到 m2dp_vec 向量中
	for(int i = 0; i < m2dp.cols()*m2dp.rows(); i++)
	{
		m2dp_vec.push_back(m2dp[i]);
	}
    // 将 m2dp_vec、robot 和 index 保存到某个位置
	save(m2dp_vec, robot, index);

	return m2dp_vec;
}

Eigen::Matrix<double, 64, 128> m2dp_descriptor::GetSignatureMatrix()
{
    //初始化thetaList和rhoList向量，用于存储一些角度和距离值。
	vector<double> thetaList(num_T_ + 1);
    // 在范围[-π, π]内均匀分布地生成角度值
	for(int i = 0; i <= num_T_; i++)
	{
		thetaList[i] = -M_PI + i * 2 * M_PI / (num_T_);
	}

	vector<double> rhoList(num_R_ + 1);
    // 在范围[0, sqrt(max_rho)]内均匀分布地生成距离值
	for(int i = 0; i <= num_R_; i++)
	{
		rhoList[i] = i * sqrt(max_rho) / num_R_;
		rhoList[i] = rhoList[i] * rhoList[i];
	}
	// make sure all points in bins 避免边界值刚好落在区间外部。
	rhoList[rhoList.size() - 1] = rhoList[rhoList.size() - 1] + 0.001;

	Eigen::Matrix<double, 64, 128> result_A;
	int index_A = 0;
	// loop on azimuth（方位角）

    // 一个嵌套循环，用于对azimuth_list和elevation_list中的每个元素进行处理，
    // 并计算特征描述矩阵的一部分
	for(int i = 0; i < azimuth_list.size();i++)
	{
        // 获取当前的azm（即azimuth_list[i]）作为当前平面的方位角
		auto azm = azimuth_list[i];
		// loop on elevation（俯仰角）
        // 对elevation_list中的每个元素进行处理，并计算特征描述矩阵的一部分
		for(int j = 0; j < elevation_list.size();j++)
		{
            // 获取当前的elv（即elevation_list[j]）作为当前平面的俯仰角
			auto elv = elevation_list[j];
			// normal vector vecN of the selected 2D plane
            // 给定的azm和elv，通过sph2cart函数将其转换为一个法线向量vecN
			Eigen::Matrix<double, 1, 3> vecN;
			sph2cart(azm, elv, 1, vecN);
			// distance of vector [1,0,0] to the surface with normal vector vecN
            // 根据法线向量vecN计算一些投影向量和坐标转换操作
			Eigen::Matrix<double, 1, 3> op(1, 0, 0);
			Eigen::MatrixXd h = op * vecN.transpose();
			// a new vector, c = h*vecN, so that vector [1,0,0]-c is the projection of x-axis onto the plane with normal vector vecN
			Eigen::Matrix<double, 1, 3> c = h(0) * vecN;
			// x-axis - c, the projection
			Eigen::Matrix<double, 1, 3> px = op - c;
			// given the normal vector vecN and the projected x-axis px, the y- axis is cross(vecN,px)
			Eigen::Matrix<double, 1, 3> py = vecN.cross(px);
			// projection of data onto space span{px,py}
            // 利用计算得到的投影向量px和py对数据进行坐标转换操作
			Eigen::MatrixXd pcx = cloud_pca * px.transpose();
			Eigen::MatrixXd pcy = cloud_pca * py.transpose();
			// pdata = np.array([pcx,pcy])
			// represent data in polar coordinates
			// vector<double> rho;
			// vector<double> theta;

            // 将数据从笛卡尔坐标系转换为极坐标系，
            // 并将转换后的点存储在名为points的std::vector<Eigen::Vector2d>类型的向量中
			std::vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> points;
            // x: rho  y: theta
			for(int i = 0; i < pcx.rows(); i++)
			{
				Eigen::Vector2d temp;
				cart2pol(pcx(i), pcy(i), temp);
				points.push_back(temp);
			}

			// main function, count points in bins
            // 定义了一个16*8大小的MatrixXd类型的矩阵hist
			Eigen::MatrixXd hist; //16*8    thetaList 17   rhoList 9
            // 将points、thetaList和rhoList作为参数传递给该函数，并将结果存储在hist矩阵中
			histogram2d(points, thetaList, rhoList, hist);
            // 将hist矩阵的每个元素除以cloud_filtered->points.size()，即点云数据的大小，实现归一化操作
			hist = hist / cloud_filtered->points.size();
			int hist_size = hist.cols() * hist.rows();
            // 将直方图矩阵hist的第i个元素复制到结果矩阵result_A的第index_A行、第i列的位置
			for (int i = 0; i < hist_size; i++)
			{
				result_A(index_A, i) = hist(i);
			}
			index_A++;
		}
	}
	return result_A;
}

// 用于坐标转换函数 球坐标到笛卡尔
void m2dp_descriptor::sph2cart(
	double azm,
	double elv,
	double r,
	Eigen::Matrix<double, 1, 3> &vecN)
{
	double x, y, z;
	x = r * cos(elv) * cos(azm);
	y = r * cos(elv) * sin(azm);
	z = r * sin(elv);
	vecN << x, y, z;
}

//用于坐标转换函数 笛卡尔到极坐标
void m2dp_descriptor::cart2pol(
	double x,
	double y,
	Eigen::Vector2d &vecN)
{
	vecN.x() = sqrt(x * x + y * y); // rho
	vecN.y() = atan2(y, x);         // phi
}
// histogram2d函数的定义，用于计算二维直方图。
void m2dp_descriptor::histogram2d(
        // 参数集points，thetaList，rhoList，hist
	std::vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> points,
	vector<double> thetaList,
	vector<double> rhoList,
	Eigen::MatrixXd &hist)
{
    //row和col表示直方图的行数和列数
	int row, col;
	row = thetaList.size() - 1;
	col = rhoList.size() - 1;
	hist = Eigen::MatrixXd::Zero(row, col);
	// Points x: rho  y: theta

    //直方图矩阵hist中存储了根据输入点集和列表计算得到的二维直方图数据
	for(auto pt : points)
	{
        //初始化行索引row_index和列索引col_index为-1
		int row_index = -1, col_index = -1;
		for(int i = 0; i <= row; i++)
		{
			if(pt.y() < thetaList[i])
			{
				row_index = i - 1;
				break;
			}
		}
		for(int j = 0; j <= col; j++)
		{
			if(pt.x() < rhoList[j])
			{
				col_index = j - 1;
				break;
			}
		}
		if(row_index >= 0 && row_index < row && col_index >= 0 && col_index < col)
		{
			hist(row_index, col_index)++;
		}
	}
}

// save函数的定义，用于保存M2DP描述符的数据。
// 可以将M2DP描述符的数据存储到m2dp_dictionaries、indexes_maps和m2dp_index_pairs中
void m2dp_descriptor::save(
	const std::vector<float> m2dp_vec,
	const int8_t robot,
	const int index)
{
	Eigen::VectorXf m2dp_mat = Eigen::Map<const Eigen::Matrix<float, -1, -1, Eigen::RowMajor>>(m2dp_vec.data(), 192, 1);
	m2dp_dictionaries[robot].push_back(m2dp_mat);
	indexes_maps[robot].push_back(m2dp_index_pairs.size());
	m2dp_index_pairs.push_back(std::make_pair(robot, index));
}
// saveDescriptorAndKey函数的定义，用于保存描述符和关键信息。
void m2dp_descriptor::saveDescriptorAndKey(
	const float* descriptorMat,
	const int8_t robot,
	const int index)
{

}

// detectInterLoopClosureID函数
std::pair<int, float> m2dp_descriptor::detectInterLoopClosureID(
	const int cur_ptr)
{
	
}
// getIndex成员函数的定义
std::pair<int8_t, int> m2dp_descriptor::getIndex(
	const int key)
{
	return m2dp_index_pairs[key];
}

//getSize函数的定义，用于获取M2DP描述符的数量
int m2dp_descriptor::getSize(
	const int id = -1)
{
	return m2dp_index_pairs.size();
}
